/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2024, USTC
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of USTC nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Xinsheng Tang */
#include "pilz_tutorial/pilz_base.h"
#include "pilz_tutorial/robot_exception.h"

manipulator::manipulator(
    ros::NodeHandle &nh,
    moveit::planning_interface::MoveGroupInterfacePtr mgptr)
    : move_group_ptr_(mgptr) {
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robotmodel_ptr_ = robot_model_loader.getModel();
  // 用于规划融合轨迹的service客户端
  seq_client_ =
      nh.serviceClient<moveit_msgs::GetMotionSequence>("/plan_sequence_path");
  seq_client_.waitForExistence();
  // 需要载入的路径点文件名称
  std::vector<std::string> station_names{"robot"};
  for (std::string &station_name : station_names) {
    load_joints(station_name);
  }
  move_group_ptr_->setPlannerId("PTP");
  move_group_ptr_->setPlanningTime(5);
}

manipulator::~manipulator() {}

void manipulator::move_single(const std::string& name, double speed, double acc, const std::string& planner) {
  move_group_ptr_->setPlannerId(planner);
  move_group_ptr_->setMaxVelocityScalingFactor(speed);
  move_group_ptr_->setMaxAccelerationScalingFactor(acc);
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  move_group_ptr_->setJointValueTarget(get_joint(name));
  if (move_group_ptr_->plan(plan) !=
      moveit::planning_interface::MoveItErrorCode::SUCCESS) {
    throw(robot_exception(std::vector<std::string>{"NO", "plan_failed"}));
  }
  move_group_ptr_->execute(plan);
}

void manipulator::move_seq(const std::vector<std::string> &V_name,
                           const std::vector<double> &V_radius,
                           const std::vector<double> &V_speed,
                           const std::vector<double> &V_acc,
                           const std::vector<std::string> &V_planner) {
  moveit_msgs::GetMotionSequence srvseq;
  srvseq.request.request =
      construct_seq_req(V_name, V_radius, V_speed, V_acc, V_planner);
  srvseq.response.response.error_code.val = 0;
  int replan_num = 5;
  int replan_count = 0;
  while (srvseq.response.response.error_code.val != 1 &&
         replan_count < replan_num) {
    seq_client_.call(srvseq);
    if (srvseq.response.response.error_code.val != 1) {
      // 将混合半径设为0，再次尝试规划
      ROS_WARN_STREAM(
          "BLENDING FAILED! Try setting blend_radius to 0 and replan...");
      // std::vector<double>(V_radius.size(), 0).swap(V_radius);
      srvseq.request.request =
          construct_seq_req(V_name, std::vector<double>(V_radius.size(), 0), V_speed, V_acc, V_planner);
    }
    ++replan_count;
  }
  if (srvseq.response.response.error_code.val != 1) {
    ROS_ERROR_STREAM("plan sequence_goal failed.ERROR CODE is"
                     << srvseq.response.response.error_code.val);
    throw(robot_exception(std::vector<std::string>{"NO", "plan_failed"}));
  }
  move_group_ptr_->execute(srvseq.response.response.planned_trajectories[0]);
}

moveit_msgs::MotionSequenceRequest manipulator::construct_seq_req(
    const std::vector<std::string> &V_name, const std::vector<double> &V_radius,
    const std::vector<double> &V_speed, const std::vector<double> &V_acc,
    const std::vector<std::string> &V_planner) {
  moveit_msgs::Constraints pose_goal;
  moveit_msgs::MotionPlanRequest freq, req;
  moveit_msgs::MotionSequenceItem sequenceItem;
  moveit_msgs::MotionSequenceRequest sequenceRequest;
  geometry_msgs::Pose curr_pose, next_pose, n_next_pose;

  move_group_ptr_->constructMotionPlanRequest(freq);
  move_group_ptr_->constructMotionPlanRequest(req);
  freq.group_name = "manipulator";
  req.group_name = "manipulator";
  freq.goal_constraints.resize(1);
  req.goal_constraints.resize(1);
  // 如果是验证，需要将上一条路径的最终路径点作为此次的起始点
  curr_pose = move_group_ptr_->getCurrentPose().pose;
  // 构造第1个MotionPlanRequest
  get_constraint(V_name[0], pose_goal, next_pose);
  freq.goal_constraints[0] = pose_goal;
  freq.max_velocity_scaling_factor = V_speed[0];
  freq.max_acceleration_scaling_factor = V_acc[0];
  freq.planner_id = V_planner[0];
  // 若多个目标路点,则计算融合半径;若只有一个目标路点,融合半径为0
  if (V_name.size() > 1) {
    get_constraint(V_name[1], pose_goal, n_next_pose);
    sequenceItem.blend_radius =
        get_blend_radius(curr_pose, next_pose, n_next_pose) * V_radius[0];
  } else {
    sequenceItem.blend_radius = 0;
  }
  sequenceItem.req = freq;
  sequenceRequest.items.emplace_back(sequenceItem);

  if (V_name.size() > 1) {
    // 构造第2~n-1个MotionPlanRequest
    int i;
    for (i = 1; i < V_name.size() - 1; ++i) {
      curr_pose = next_pose;
      next_pose = n_next_pose;
      req.goal_constraints[0] = pose_goal;
      req.max_velocity_scaling_factor = V_speed[i];
      req.max_acceleration_scaling_factor = V_acc[i];
      req.planner_id = V_planner[i];
      get_constraint(V_name[i + 1], pose_goal, n_next_pose);
      sequenceItem.blend_radius =
          get_blend_radius(curr_pose, next_pose, n_next_pose) * V_radius[i];
      sequenceItem.req = req;
      sequenceRequest.items.emplace_back(sequenceItem);
    }
    // 构造第n个MotionPlanRequest
    req.goal_constraints[0] = pose_goal;
    req.max_velocity_scaling_factor = V_speed[i];
    req.max_acceleration_scaling_factor = V_acc[i];
    req.planner_id = V_planner[i];
    sequenceItem.blend_radius = 0.0;
    sequenceItem.req = req;
    sequenceRequest.items.emplace_back(sequenceItem);
  }
  return sequenceRequest;
}

void manipulator::get_constraint(const std::string& name,
                                 moveit_msgs::Constraints &pose_goal,
                                 geometry_msgs::Pose &next_pose) {
  robot_state::RobotState rstate(robotmodel_ptr_);
  rstate.setToDefaultValues();
  rstate.setVariablePositions(joint_names_, get_joint(name));
  pose_goal = kinematic_constraints::constructGoalConstraints(
      rstate, robotmodel_ptr_->getJointModelGroup("manipulator"));
  Eigen::Isometry3d mat = rstate.getGlobalLinkTransform("tool0");
  next_pose = toPose(mat);
}

double manipulator::get_blend_radius(const geometry_msgs::Pose &curr_pose,
                                     const geometry_msgs::Pose &next_pose,
                                     const geometry_msgs::Pose &n_next_pose) {
  // 若curr_pose和n_next_pose重合,将融合半径置0
  if (pow(n_next_pose.position.x - curr_pose.position.x, 2) +
          pow(n_next_pose.position.y - curr_pose.position.y, 2) +
          pow(n_next_pose.position.z - curr_pose.position.z, 2) <
      0.0025)
    return 0.0;
  double radius1 = sqrt(pow(next_pose.position.x - curr_pose.position.x, 2) +
                        pow(next_pose.position.y - curr_pose.position.y, 2) +
                        pow(next_pose.position.z - curr_pose.position.z, 2));
  double radius2 = sqrt(pow(n_next_pose.position.x - next_pose.position.x, 2) +
                        pow(n_next_pose.position.y - next_pose.position.y, 2) +
                        pow(n_next_pose.position.z - next_pose.position.z, 2));
  double radius = std::min(radius1, radius2);
  if (radius < 0.02) return 0.0;
  return radius;
}


void manipulator::load_joints(const std::string &station_name) {
  std::string file_root = ros::package::getPath("pilz_tutorial") + "/data/";
  Json::Reader jsonreader;
  Json::Value root;
  std::string file_path = file_root + station_name + ".json";
  std::ifstream file(file_path, std::ios::binary);
  if (jsonreader.parse(file, root)) {
    // 获取关节角
    Json::Value value = root["joints"];
    std::string name;
    for (int i = 0; i < value.size(); ++i) {
      name = value[i]["name"].asString();
      joints_[name].resize(6);
      for (int j = 0; j < 6; ++j)
        joints_[name].at(j) = value[i]["value"][j].asDouble();
    }
  }
  file.close();
}

std::vector<double> manipulator::get_joint(const std::string &name) {
  auto it = joints_.find(name);
  if (it == joints_.end()) {
    ROS_ERROR_STREAM("no_joints_value:" + name);
    throw(robot_exception(std::vector<std::string>{"NO", "internal_error"}));
  }
  return (it->second);
}

geometry_msgs::Pose toPose(const Eigen::Isometry3d &T) {
  Eigen::Vector3d X(T.translation());
  Eigen::Quaterniond Q(T.rotation());
  geometry_msgs::Pose pose;
  pose.position.x = X[0];
  pose.position.y = X[1];
  pose.position.z = X[2];
  pose.orientation.x = Q.coeffs()[0];
  pose.orientation.y = Q.coeffs()[1];
  pose.orientation.z = Q.coeffs()[2];
  pose.orientation.w = Q.coeffs()[3];
  return pose;
}